//
// Created by msi on 2025/8/8.
//

#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>
#include <string>
#include <algorithm>
#include "global_defination/global_defination.h"

int main() {
    std::ifstream infile(WORK_SPACE_PATH + "/csv_test/csv/102/20250728_020915.csv");
    std::ofstream outfile(WORK_SPACE_PATH + "/csv_test/csv/102/20250728_020915_filter.csv");
    std::string line;
    std::vector<std::string> keep_columns = {
            "seconds_from_epoch", "nanoseconds",
            "body_rpy_0", "body_rpy_1", "body_rpy_2",
            "rpy_setpoint_0", "rpy_setpoint_1", "rpy_setpoint_2",
            "gyro_corrected_0", "gyro_corrected_1", "gyro_corrected_2",
            "Torque_0", "Torque_1", "Torque_2",
            "Torque_body_fb_0", "Torque_body_fb_1", "Torque_body_fb_2",
            "motor_currents[0]", "motor_currents[1]", "motor_currents[2]",
            "sensor_data.motor_rpm_speeds_[0]", "sensor_data.motor_rpm_speeds_[1]", "sensor_data.motor_rpm_speeds_[2]",

    };
    std::vector<int> col_indices;
    bool header_processed = false;

    while (std::getline(infile, line)) {
        std::stringstream ss(line);
        std::string cell;
        std::vector<std::string> cells;
        while (std::getline(ss, cell, ',')) {
            cells.push_back(cell);
        }

        if (!header_processed) {
            // 找到需要的列索引
            for (size_t i = 0; i < cells.size(); ++i) {
                if (std::find(keep_columns.begin(), keep_columns.end(), cells[i]) != keep_columns.end()) {
                    col_indices.push_back(i);
                }
            }
            // 输出新表头
            for (size_t i = 0; i < col_indices.size(); ++i) {
                outfile << cells[col_indices[i]];
                if (i < col_indices.size() - 1) outfile << ",";
            }
            outfile << "\n";
            header_processed = true;
        } else {
            // 输出需要的列
            for (size_t i = 0; i < col_indices.size(); ++i) {
                outfile << cells[col_indices[i]];
                if (i < col_indices.size() - 1) outfile << ",";
            }
            outfile << "\n";
        }
    }
    infile.close();
    outfile.close();
    return 0;
}
